#include <Wire.h>
// Wire library uses 7-bit addresses and automatically
// sets the r/w bit
static const uint8_t ITG3200 = 0x68;
static const uint8_t SMPLRT_DIV = 0x15;
static const uint8_t DLPF_FS = 0x16;
static   const uint8_t BIT_FS_2000 = 0x18;
static   const uint8_t BIT_DLPF_256 = 0x00;
static   const uint8_t BIT_DLPF_188 = 0x01;
static   const uint8_t BIT_DLPF_98 = 0x02;
static   const uint8_t BIT_DLPF_42 = 0x03;
static   const uint8_t BIT_DLPF_20 = 0x04;
static   const uint8_t BIT_DLPF_10 = 0x05;
static   const uint8_t BIT_DLPF_5 = 0x06;
static const uint8_t INT_CFG = 0x17;
static   const uint8_t BIT_ACTL = 0x80;
static   const uint8_t BIT_OPEN = 0x40;
static   const uint8_t BIT_LATCH_INT_EN = 0x20;
static   const uint8_t BIT_INT_ANYRD_2CLEAR = 0x10;
static   const uint8_t BIT_ITG_RDY_EN = 0x04;
static   const uint8_t BIT_RAW_RDY_EN = 0x01;
static const uint8_t INT_STATUS = 0x1A;
static   const uint8_t BIT_ITG_RDY = 0x04;
static   const uint8_t BIT_RAW_DATA_RDY = 0x01;
static const uint8_t TEMP_OUT_H = 0x1B;
static const uint8_t TEMP_OUT_L = 0x1C;
static const uint8_t GYRO_XOUT_H = 0x1D;
static const uint8_t GYRO_XOUT_L = 0x1E;
static const uint8_t GYRO_YOUT_H = 0x1F;
static const uint8_t GYRO_YOUT_L = 0x20;
static const uint8_t GYRO_ZOUT_H = 0x21;
static const uint8_t GYRO_ZOUT_L = 0x22;
static const uint8_t PWR_MGM = 0x3E;
static   const uint8_t BIT_H_RESET = 0x80;
static   const uint8_t BIT_SLEEP = 0x40;
static   const uint8_t BIT_STBY_XG = 0x20;
static   const uint8_t BIT_STBY_YG = 0x10;
static   const uint8_t BIT_STBY_ZG = 0x08;
static   const uint8_t BIT_CLK_INT = 0x00;
static   const uint8_t BIT_CLK_PLL_X = 0x01;
static   const uint8_t BIT_CLK_PLL_Y = 0x02;
static   const uint8_t BIT_CLK_PLL_Z = 0x03;
static   const uint8_t BIT_CLK_PLL_E32K = 0x04;
static   const uint8_t BIT_CLK_PLL_E19M = 0x05;

void writeITG(uint8_t reg, uint8_t val)
{
  static const uint8_t writeAddr = ITG3200 << 1;
  Wire.beginTransmission(ITG3200);
  Wire.send(reg);
  Wire.send(val);
  Wire.endTransmission();
  // print the message, stuck here to make sure I was matching
  // the datasheet.
  Serial.print("0x");
  Serial.print(writeAddr, HEX);
  Serial.print(" 0x");
  Serial.print(reg, HEX);
  Serial.print(" 0x");
  Serial.println(val, HEX);
}

void getAndDump(int16_t *acc)
{
  // Like the HMC5883L, we need to flip the bytes.
  // Easiest way is to store the data in reverse order.
  // 8 bytes gets us the LSB and MSB of the temperature and gyros
  Wire.requestFrom(ITG3200, (uint8_t)8);
  // pointer to use to store the data. LSB comes first.
  byte *p = (byte*)&(acc[3]);
  *p++; // last byte
  // wait for 8 bytes to be available
  while (Wire.available() < 8) { 
  }
  for (int i = 0; i < 8; i++)
  {
    *p = Wire.receive();
    p--; // advance the pointer to the next 8 bits
  }
  Serial.print(acc[3]); // finally, print the quantities
  Serial.print(" ");
  Serial.print(acc[2]);
  Serial.print(" ");
  Serial.print(acc[1]);
  Serial.print(" ");
  Serial.println(acc[0]);
}

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  // Set the divider so that the rate matches the HMC5883L
  Serial.print("SMPLRT_DIV ");
  writeITG(SMPLRT_DIV, 65);
  Serial.print("DLPF_FS ");
  writeITG(DLPF_FS, BIT_FS_2000 | BIT_DLPF_188);
  // Put the read address on the bus
  Wire.beginTransmission(ITG3200);
  Wire.send(TEMP_OUT_H);
  Wire.endTransmission();
}

void loop()
{
  int16_t data[4];
  getAndDump(data);
  // point to the first data register
  Wire.beginTransmission(ITG3200);
  Wire.send(TEMP_OUT_H);
  Wire.endTransmission();
  // sleep long enough for the next set of measurements (@15Hz)
  delay(67);
}



